#include "stdio.h"
#include "odom.h"
#include <pthread.h>

ROBOT_POSITION  robot_position;


const double odmo_plus2distance = 0.0005125;    //距离脉冲比：0.0005125 cm/p
const double odom_distance2plus = 2000;         //脉冲距离比:2000

/*
 *	机器人位置表示：基地址+偏移
 *	基地址：RFID标签号
 *	偏移  ：里程计
 */
void robot_position_updata(int32_t step_plus)
{	
	if(step_plus == 0)	return;	
	if(step_plus < 0) step_plus *= -1;

    pthread_mutex_lock(&robot_position.odom_lock);
    
    robot_position.plus += step_plus;
    robot_position.dist_to_radpoint = (int32_t)(odmo_plus2distance * robot_position.plus);

    //printf("distance: %d \n",robot_position.dist_to_radpoint);

    pthread_mutex_unlock(&robot_position.odom_lock);

}

ROBOT_POSITION *read_odom(ROBOT_POSITION *position)
{
    pthread_mutex_lock(&robot_position.odom_lock);
    
    position->dual = robot_position.dual;
    position->plus = robot_position.plus;
    position->dist_to_radpoint = robot_position.dist_to_radpoint;

    pthread_mutex_unlock(&robot_position.odom_lock);

    return position;
}



void write_odom(uint8_t dual, int32_t distance)
{

    pthread_mutex_lock(&robot_position.odom_lock);
    
    robot_position.dual = dual;
    robot_position.plus = distance * odom_distance2plus;
    robot_position.dist_to_radpoint = distance;

    pthread_mutex_unlock(&robot_position.odom_lock);

}



